﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using OX.Copyable;
using OpenNI;
using BTKinect.DataType;

namespace BTKinect
{
    class KinectBasicCalculations
    {
        #region MEMBERS / CONSTRUCTORS
        private bool gotDepthGenerator = false;
        private DepthGenerator _depth;
        public KinectBasicCalculations(DepthGenerator depth)
        {
            _depth = depth;
            gotDepthGenerator = true;
        }
        public KinectBasicCalculations()
        {
            gotDepthGenerator = false;
        }
        #endregion

        #region METHODS
        /// <summary>
        /// Calculate cartesian coordinates
        /// </summary>
        /// <param name="pnt"></param>
        /// <returns></returns>
        public KinectPoint3D CalculateRealValues(KinectPoint3D pnt)
        {
            Point3D _pnt = pnt;
            if (gotDepthGenerator)
                return _depth.ConvertProjectiveToRealWorld(_pnt);
            else
                throw new ArgumentException("Geen Depthgenerator opgegeven (zie KinectBasicCalcultions.cs)");
        }

        /// <summary>
        /// asldfjasklfjasklfjlaksdjfasfjasdlfjaklsdjfaklsdjfklasjdfkalsdjfklasjf
        /// </summary>
        /// <param name="dicKinectCoor"></param>
        /// <returns></returns>
        public Dictionary<SkeletonJoint, KinectPoint3D> CalculateRealValuesDictionary(Dictionary<SkeletonJoint, SkeletonJointPosition> dicKinectCoor)
        {
            Dictionary<SkeletonJoint, KinectPoint3D> d = new Dictionary<SkeletonJoint, KinectPoint3D>();
            foreach (var item in dicKinectCoor)
            {
                d.Add(item.Key, CalculateRealValues((KinectPoint3D)item.Value.Position));
            }
            return (Dictionary<SkeletonJoint, KinectPoint3D>)d.Copy();
        }

        /// <summary>
        /// Calculates the angle between two vectors (three 3DPoints: pnt1, center, pnt2)
        /// </summary>
        /// <param name="point1"></param>
        /// <param name="pointCenter"></param>
        /// <param name="point2"></param>
        /// <returns></returns>
        public float CalculateAngle(KinectPoint3D point1, KinectPoint3D pointCenter, KinectPoint3D point2)
        {
            //temporary variables
            float lengthVector1, lengthVector2, _dotProduct;

            //determine length
            lengthVector1 = (float)point1.Distance(pointCenter);
            lengthVector2 = (float)point2.Distance(pointCenter);

            //create vectors
            KinectVector vector1 = point1 - pointCenter;
            KinectVector vector2 = point2 - pointCenter;

            //calculate dot Product
            _dotProduct = (float)((vector1[0] * vector2[0]) + (vector1[1] * vector2[1]) + (vector1[2] * vector2[2]));

            return RadianToDegree(Math.Acos(_dotProduct / (lengthVector1 * lengthVector2)));
        }
        /// <summary>
        /// Converts radian to degrees
        /// </summary>
        /// <param name="angle"></param>
        /// <returns></returns>
        public float RadianToDegree(double angleInRadian)
        {
            return (float)(angleInRadian * (180.0 / Math.PI));
        }
        /// <summary>
        /// Converts degrees to radian
        /// </summary>
        /// <param name="angleInDegrees"></param>
        /// <returns></returns>
        public float DegreeToRadian(double angleInDegrees)
        {
            return (float)(angleInDegrees * (Math.PI / 180.0));
        }
        /// <summary>
        /// needs a list with 3 KinectPoint3D (2 segments) returns an array with 2 elements containing
        /// the average length of segment 1 and segment 2
        /// </summary>
        /// <param name="lst"></param>
        /// <returns></returns>
        public double[] calculateAverageLength(List<KinectPoint3D[]> lst)
        {
            //temp var
            List<double> lengthSegment1 = new List<double>(lst.Count);
            List<double> lengthSegment2 = new List<double>(lst.Count);

            //get Point3D from list and calculate lengt, put length in list
            foreach (var item in lst)
            {
                lengthSegment1.Add(item[0].Distance(item[1]));
                lengthSegment2.Add(item[1].Distance(item[2]));
            }

            //average length
            double[] meanLength = new double[2];
            meanLength[0] = lengthSegment1.Average();
            meanLength[1] = lengthSegment2.Average();

            return meanLength;
        }
        /// <summary>
        /// Round a KinectPoint3D
        /// </summary>
        /// <param name="k"></param>
        /// <param name="numberOfDecimals"></param>
        /// <returns></returns>
        #endregion
    }
}
